/*  -------------------------- (C) COPYRIGHT 2022 Fortiortech ShenZhen ---------------------------*/
/**
    @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
    @file      xxx.c
    @author    Fortiortech  Appliction Team
    @since     Create:2022-07-14
    @date      Last modify:2022-07-14
    @note      Last modify author is Leo.li
    @brief
*/

/* Includes -------------------------------------------------------------------------------------*/
#include <FU68xx_2.h>
#include <Myproject.h>

bool data isCtrlPowOn;
OUTLOOP xdata SpeedPICtrl;
FOCCTRL xdata mcFocCtrl;
ONVarible xdata ONOFFTest;
SLEEPMODE xdata SleepSet;
MCRAMP xdata mcRefRamp; ///< 控制指令爬坡结构体相关变量
MC_CONTROL xdata MCCtrl;

/**
    @brief        切入控制闭环时变量初始化赋值
    @date         2022-07-13
*/
void OutLoopParameterSet(void)
{
    memset(&SpeedPICtrl, 0, sizeof(OUTLOOP)); // SpeedControl clear
    mcFocCtrl.SpeedLoopTime = SPEED_LOOP_TIME;
    SpeedPICtrl.ExtKP = SKP;
    SpeedPICtrl.ExtKI = SKI;
    // SpeedPICtrl.ExtOutMax = SOUTMAX;
    SpeedPICtrl.ExtOutMax = SOUTMAX0;
    SpeedPICtrl.ExtOutMin = SOUTMIN;
    PI1_KP = SpeedPICtrl.ExtKP;
    PI1_KI = SpeedPICtrl.ExtKI;
    PI1_UKMAX = SpeedPICtrl.ExtOutMax;
    PI1_UKMIN = SpeedPICtrl.ExtOutMin;
    PI1_UKH = IQ_RUN_CURRENT;
    PI1_EK1 = 500;
    PI2_KP = _Q12(0.1); // 限制功率调节
    PI2_KI = _Q15(0.008);
    PI2_UKMAX = SOUTMAX;
    PI2_UKMIN = SOUTMAX >> 2;
    PI2_UKH = PI2_UKMAX; // 输出从最大开始调节
#if (MotorFiledWeakenEn)
    {
        FiledWeakenInit();
    }
#endif
}

/**
    @brief        对变量取16位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint16 Abs_F16(int16 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
    @brief        对变量取32位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
    @brief        启停测试工具，用于测试启动可靠性
    @date         2022-07-14
*/
void ONOFF_Starttest(void)
{
    if (ONOFFTest.ONOFF_Flag == 1)
    {
        ONOFFTest.ON_Count++;

        if (ONOFFTest.ON_Count > StartON_Time)
        {
            ONOFFTest.ON_Count = 0;
            ONOFFTest.ONOFF_Times++;
            ONOFFTest.ONOFF_Flag = 0;
            MCCtrl.FlagONOFF = 0;
            MCCtrl.TargetValue = 0;
        }
    }
    else
    {
        if (mcState != mcFault)
        {
            ONOFFTest.OFF_Count++;

            if (ONOFFTest.OFF_Count > StartOFF_Time)
            {
                ONOFFTest.OFF_Count = 0;
                ONOFFTest.ONOFF_Flag = 1;
                MCCtrl.FlagONOFF = 1;
                MCCtrl.PowerLimitValue = POWERLPFLIMIT;
                MCCtrl.TargetValue = Motor_Speed_HIgh;
            }
        }
    }
}

/**
    @brief        外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
                 建议使用默认1ms周期运行
    @date         2022-07-14
*/
void Speed_response(void)
{
    static uint8 F5SEG_Flag = 0;
    /****功率滤波*****/
    if (mcState == mcRun)
    {
        mcFocCtrl.CurrentPower = FOC__POW << 3;
        mcFocCtrl.Powerlpf = LPFFunction(mcFocCtrl.CurrentPower, mcFocCtrl.Powerlpf, 10); // 注意低通滤波器系数范围为0---127
    }

    /****速度滤波、反电动势滤波*****/
    if ((mcState != mcInit) && (mcState != mcReady))
    {
        mcFocCtrl.SpeedFlt = LPFFunction(FOC__EOME, mcFocCtrl.SpeedFlt, 10); // 注意低通滤波器系数范围为0---127
        mcFocCtrl.EsValue = LPFFunction(FOC__EMF, mcFocCtrl.EsValue, 10);
    }
    else
    {
        mcFocCtrl.SpeedFlt = 0;
    }

    /****UQ电压值滤波****/
    mcFocCtrl.UqFlt = LPFFunction(FOC__UQ, mcFocCtrl.UqFlt, 10); // UQ值
    mcFocCtrl.UdFlt = LPFFunction(FOC__UD, mcFocCtrl.UdFlt, 10); // UD值

    if ((mcState == mcRun) || (mcState == mcStop))
    {
        switch (mcFocCtrl.CtrlMode)
        {
        case 0:
        {
#if (START_MODE == PLL_Start)

            if ((FOC__EOME > Motor_Loop_Speed) && (PLLfunction.PLLFunctionFlag > 2))
#else
            if (FOC__EOME > Motor_Loop_Speed)
#endif
            {
                FOC_THECOR = 0x04;
                FOC_IDREF = ID_RUN_CURRENT;

                FOC_DQKP = DQKP;
                FOC_DQKI = DQKI;
                FOC_EKP = OBSW_KP_GAIN_RUN3;      // 估算器里的PI的KP
                FOC_EKI = OBSW_KI_GAIN_RUN3;      // 估算器里的PI的KI
                FOC_THECOMP = _Q15(15.0 / 180.0); // SMO 估算补偿角
#if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                {
                    mcRefRamp.OutValue_float = mcFocCtrl.SpeedFlt; // 防止mcSpeedRamp.ActualValue从0开始爬坡，造成估算异常，电流异常
                }
#elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                {
                    mcRefRamp.OutValue_float = mcFocCtrl.Powerlpf;
                }
#elif (Motor_Speed_Control_Mode == CURRENT_LOOP_CONTROL)
                {
                    mcFocCtrl.mcIqref = FOC_IQREF;
                }
#endif
                mcRefRamp.IncValue = SPEEDRAMPINC; // 运行速度环增量
                mcRefRamp.DecValue = SPEEDRAMPDEC;
                OutLoopParameterSet(); // 环路基本参数设置
                mcFocCtrl.CtrlMode = 1;
                //                    FOC_IQREF = IQ_RUN_CURRENT>>1;
            }
        }
        break;

        case 1:
        {
#if (OUTLoop_Mode == OUTLoop_Disable)
            {
                mcFocCtrl.mcIqref = FOC_IQREF;

                if (FOC_IQREF < QOUTVALUE)
                {
                    mcFocCtrl.mcIqref += QOUTINC;

                    if (mcFocCtrl.mcIqref > QOUTVALUE)
                    {
                        mcFocCtrl.mcIqref = QOUTVALUE;
                    }

                    FOC_IQREF = mcFocCtrl.mcIqref;
                }
                else if (FOC_IQREF > QOUTVALUE)
                {
                    mcFocCtrl.mcIqref -= QOUTINC;

                    if (mcFocCtrl.mcIqref < QOUTVALUE)
                    {
                        mcFocCtrl.mcIqref = QOUTVALUE;
                    }

                    FOC_IQREF = mcFocCtrl.mcIqref;
                }
            }
#elif (OUTLoop_Mode == OUTLoop_Enable)
            {
                mcFocCtrl.SpeedLoopTime++;

                if (mcFocCtrl.SpeedLoopTime > SPEED_LOOP_TIME) // 外环调节周期
                {
                    MCCtrl.ActualValue = Motor_Ramp(MCCtrl.TargetValue); // 爬坡增量调节
                    mcFocCtrl.SpeedLoopTime = 0;
#if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                    {
                        SpeedPICtrl.ExtEK = MCCtrl.ActualValue - mcFocCtrl.SpeedFlt;
                        SpeedPICtrl.ExtOut = HW_One_PI(SpeedPICtrl.ExtEK);                                // 速度环调节
                        SpeedPICtrl.LimitIqOut = HW_One_PI2(MCCtrl.PowerLimitValue - mcFocCtrl.Powerlpf); // 限制功率

                        if ((SpeedPICtrl.LimitIqOut < SpeedPICtrl.ExtOut)) // 限制输出电流
                        {
                            SpeedPICtrl.ExtDec = (SpeedPICtrl.ExtIqOut - SpeedPICtrl.LimitIqOut) / 3;
                            SpeedPICtrl.ExtIqOut -= SpeedPICtrl.ExtDec;
                            //                                MCCtrl.ActualValue = mcFocCtrl.SpeedFlt;
                            //                                SpeedPICtrl.LimitFlag = 1;
                        }
                        else
                        {
                            SpeedPICtrl.ExtIqOut = SpeedPICtrl.ExtOut;
                            //                                SpeedPICtrl.LimitFlag = 0;
                        }

                        SpeedPICtrl.ExtOut = SpeedPICtrl.ExtIqOut;
#if (MotorFiledWeakenEn)
                        {
                            FileWeakenControl();
                        }
#else
                        {
                            FOC_IQREF = SpeedPICtrl.ExtIqOut;
                        }
#endif
                    }
#elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                    {
                        FOC_IQREF = HW_One_PI(MCCtrl.ActualValue - mcFocCtrl.Powerlpf);
                    }
#elif (Motor_Speed_Control_Mode == CURRENT_LOOP_CONTROL)
                    {
                        if (mcFocCtrl.mcIqref < SOUTMAX)
                        {
                            mcFocCtrl.mcIqref += 2;
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        else
                        {
                            FOC_IQREF = SOUTMAX;
                        }
                    }
#endif
                }
                if (PI1_UKMAX < SOUTMAX)
                {
                    PI1_UKMAX += 1;
                }
            }
#endif
#if (SVPWM_5_Segment_Run_Enale == 1)
            {
                if ((mcFocCtrl.SpeedFlt > Motor_F5SEG_Speed) && (F5SEG_Flag == 0)) // S_Value(73500)
                {
                    SetBit(FOC_CR2, F5SEG);
                    F5SEG_Flag = 1;
                }
                else if ((mcFocCtrl.SpeedFlt < Motor_F7SEG_Speed) && (F5SEG_Flag == 0)) // S_Value(71500)
                {
                    ClrBit(FOC_CR2, F5SEG);
                    F5SEG_Flag = 0;
                }
            }
#endif
#if (START_MODE == PLL_Start)
            {
                if (FOC_EK3 > OBS_K3T1 + 50)
                {
                    FOC_EK3 -= 50;
                }
                else if (FOC_EK3 < OBS_K3T1 - 50)
                {
                    FOC_EK3 += 50;
                }
                else
                {
                    FOC_EK3 = OBS_K3T1;
                }
            }
#endif
        }
        break;
        }
    }
}

/**
    @brief        控制给定爬坡函数
                 以浮点进行计算，解决整数爬坡由于精度的影响，导致爬坡结果阶梯变化
                 函数控制周期默认为闭环控制周期，建议使用默认1ms周期运行
    @param[in]    ref 给定目标值
    @return       爬坡结果（int16）
    @date         2022-07-14
*/
int16 Motor_Ramp(int16 ref)
{
    mcRefRamp.RefValue = ref; // 爬坡函数输入

    if (mcRefRamp.OutValue_float < mcRefRamp.RefValue)
    {
        if (mcRefRamp.OutValue_float + mcRefRamp.IncValue < mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float += mcRefRamp.IncValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }
    else
    {
        if (mcRefRamp.OutValue_float - mcRefRamp.DecValue > mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float -= mcRefRamp.DecValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }

    return (int16)mcRefRamp.OutValue_float; // 输出浮点数取整
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void Sleepmode(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 睡眠模式测试
    /*---------------------------------------------------------------------------*/
void Sleepmode(void)
{
    SleepSet.SleepDelayCout++;

    if (SleepSet.SleepDelayCout >= 20000) // 最大65530，若要再大，需改数据类型
    {
        //        mcSpeedRamp.TargetValue = 0;
        //        MOE     = 0;
        //        ClrBit(DRV_CR, FOCEN);  //关闭FOC
        //        SleepSet.SleepDelayCout = 0;
        //        SleepSet.SleepFlag = 1;
        //        SetBit(P1_IE, P11);   // config P11 as the source of EXTI1
        //        SetBit(PCON, STOP);
    }
}
/*---------------------------------------------------------------------------*/
/*  Name     :   void StarRampDealwith(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description:
    /*---------------------------------------------------------------------------*/
void StarRampDealwith(void)
{
    if ((mcState == mcRun))
    {
#if (START_MODE == PLL_Start)
        {
            RampFucntion();
        }
#else
        {
            if (mcFocCtrl.CtrlMode == 0)
            {
                if (mcFocCtrl.mcIqref < IQ_Start_CURRENT)
                {
                    mcFocCtrl.mcIqref += 1;
                }
                else
                {
                    mcFocCtrl.mcIqref = IQ_Start_CURRENT;
                }

                FOC_IQREF = mcFocCtrl.mcIqref;
            }
        }
#endif

        if (mcFocCtrl.State_Count == 100) // 2300
        {
            ClrBit(FOC_CR2, UQD);
            FOC_EKP = OBSW_KP_GAIN_RUN; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 80) // 2000
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 40) // 1600
        {
            FOC_EKP = OBSW_KP_GAIN_RUN2; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2; // 估算器里的PI的KI
        }
        else
            ;
    }
}

/**
    @brief        调速信号处理包含：开关机控制、将调速信号处理成控制目标给定信号
    @date         2022-07-14
*/
void TargetSpeed_Colletion(void)
{
#if (SPEED_MODE == ONOFFTEST)
    {
        ONOFF_Starttest();
    }
#elif (SPEED_MODE == KEYSCANMODE)
    {
        //       if (mcFocCtrl.mcDcbusFlt > _Q15(200 / HW_BOARD_VOLT_MAX))
        //        {
        MCCtrl.PowerLimitValue = POWERLPFLIMIT;
        //            PI1_UKMAX = SOUTMAX;
        //        }
        //        else
        //        {
        //          MCCtrl.PowerLimitValue = POWERLPFLIMIT*0.5;
        //         FOC_QMAX            = QOUTMAX>>2;
        //         MCCtrl.TargetValue = Motor_Speed_HIgh>>1;
        //        }
        // MCCtrl.FlagONOFF   = 1;
        KeyScan(); // 获取按键值
    }
#else
    {
        MCCtrl.PowerLimitValue = POWERLPFLIMIT;
        MCCtrl.TargetValue = S_Value(104000); // 转速赋值
        MCCtrl.FlagONOFF = 1;
    }
#endif
#if (FREnable == 1)
    {
        FRControl();
    }
#endif
}

/**
    @brief        FR控制逻辑，反转存在时间控制，不会一直反转
    @date         2022-07-14
*/
void FRControl(void)
{
    static uint16 CWCCWDelay = 0;

    if ((FRPin != MCCtrl.FR_Status) && (MCCtrl.FlagFR == 0)) // CCW
    {
        if (CWCCWDelay++ == 500)
        {
            CWCCWDelay = 0;
            MCCtrl.FlagFR = 1;
            MCCtrl.FR_Status = FRPin;
        }
    }

    if (MCCtrl.FlagFR)
    {
        MCCtrl.FlagONOFF = 0;
    }
}

/**
    @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
                 该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
    @date         2022-07-14
*/
void TickCycle_1ms(void)
{
    uint16 Temperature = 0;
    uint16 TempKeyValue = 0;
    /* **********温度采集********** */
    // Temperature = ADC11_DR << 3;
    // User.Temperature = LPFFunction(Temperature, User.Temperature, 30);
    User.Temperature = ADC11_DR << 3;
    /* **********按键采集********** */
    // TempKeyValue = ADC7_DR << 3; // 档位采样
    // KS.KeyADCValue = LPFFunction(TempKeyValue, KS.KeyADCValue, 30);
    KS.KeyADCValue = ADC7_DR << 3; // 档位采样
    /*****DCbus的采样获取值并滤波******/
    mcFocCtrl.mcDcbusFlt = ADC2_DR << 3;
    mcFocCtrl.mcIbusFlt = LPFFunction(ADC11_DR << 3, mcFocCtrl.mcIbusFlt, 2);
    mcFocCtrl.CurrentPower1 = (uint32)mcFocCtrl.mcDcbusFlt * (uint32)mcFocCtrl.mcIbusFlt;
    /* -----环路响应，如速度环、转矩环、功率环等----- */
    Speed_response();
    /*****故障保护函数功能，如过欠压保护、启动保护、缺相、堵转等********/
    Fault_Detection();
    /*****电机启动爬坡函数处理*****/
    StarRampDealwith();
    /*****获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE,KEYSCANMODE)的目标值修改******/
    TargetSpeed_Colletion();
#if (SPEED_MODE == KEYSCANMODE)
    led1msBreath(); // 呼吸灯

    /*****LED显示******/
    if (KS.ChangeKeyFlg && mcFocCtrl.ZeroLossFlag == 0) // 更新LED显示
    {
        LEDControl();
        KS.ChangeKeyFlg = 0;
    }

#endif

    /*****电机状态机的时序处理*****/
    if (mcFocCtrl.State_Count > 0)
    {
        mcFocCtrl.State_Count--;
    }

    if ((mcState == mcRun) && (User.TPCtrlDealy > 0))
    {
        User.TPCtrlDealy--;
    }

#if (TailWind_Mode == TailWind)
#if (FRDetectMethod == FOCMethod)
    {
        FOCTailWindTimeLimit();
    }
#else
    {
        if (BEMFDetect.BEMFTimeCount > 0)
        {
            BEMFDetect.BEMFTimeCount--;
        }
    }
#endif
#endif
}
